package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.spot.SensorServer;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.spot.DemoSwitchListener;
import com.grt192.mechanism.spot.SLight;
import com.grt192.sensor.spot.GRTCompass;
import com.grt192.sensor.spot.GRTDemoSwitch;
import com.grt192.sensor.spot.IDG500Gyro;
import com.grt192.utils.Util;

/**
 * 
 * @author ajc
 */
public class IRobotDriverSpotMacro extends GRTObject implements Macro,
		TelemetryPorts, SensorChangeListener, DemoSwitchListener {

	public IRobotDriverSpotMacro() {
		setPrinting(true);
	}

	public int execute() {

		while (true) {
			Util.sleep(1000);
		}
	}

	GRTDemoSwitch[] switches;

	public void startSwitch() {
		switches = new GRTDemoSwitch[2];
		for (int i = 0; i < switches.length; i++) {
			switches[i] = new GRTDemoSwitch(0, 50, "Swtich" + 0);
			switches[i].addSwitchListener(this);
			switches[i].start();
		}
	}

	/*
	 * 6 lights available: compass, gyro, create 1 for each
	 */
	private SLight[] lights = SLight.get();
	private int compassLight = 3;
	private int gyroLight = 4;

	private void initLights() {
		lights[compassLight].rawColor(GRTDemoLED.Color.CHARTREUSE);
		lights[gyroLight].rawColor(GRTDemoLED.Color.CHARTREUSE);

	}

	private static final int RATE_PIN = 0;
	private static final int REF_PIN = 1;
	private static final int TEMP_PIN = 2;
	private static final double CHANGE_THRESHOLD = .5;
	IDG500Gyro gyro;
	GRTCompass compass;
	SensorServer forGyro;
	SensorServer forCompass;

	public void startSensors() {
		gyro = new IDG500Gyro(RATE_PIN, REF_PIN, TEMP_PIN, 20, "gyro");
		gyro.addSensorChangeListener(this);
		gyro.start();

		compass = new GRTCompass(CHANGE_THRESHOLD, 20, "compass");
		compass.addSensorChangeListener(this);
		compass.start();

		forGyro = new SensorServer(gyro, GYRO_PORT);
		forGyro.start();

		forCompass = new SensorServer(compass, COMPASS_PORT);
		compass.start();
	}

	public void sensorStateChanged(SensorEvent e, String key) {
		if (e.getSource() == gyro) {
			lights[compassLight].blinkToBlack();
		} else if (e.getSource() == compass) {
			log("compass heading " + e.getData(key));
			lights[gyroLight].blinkToBlack();

		}
	}

	public void switchPressed(GRTDemoSwitch source) {
	}

	public void switchReleased(GRTDemoSwitch source) {
	}
}
